#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <action_msgs/msg/goal_status.hpp>
#include <nav2_msgs/action/compute_path_to_pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

class GlobalPlanner : public rclcpp::Node {
public:
    GlobalPlanner() : Node("global_planner") {
        // 订阅/goal_pose话题，用于全局路径规划
        goal_subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/goal_pose", 10, std::bind(&GlobalPlanner::GoalPoseCallback, this, std::placeholders::_1));

        // 创建路径规划动作客户端，实现全局路径规划功能
        path_action_client_ =
            rclcpp_action::create_client<nav2_msgs::action::ComputePathToPose>(this, "compute_path_to_pose");
    }

private:
    // 接收到/goal_pose消息后，调用路径规划动作服务器，规划全局路径, 并会发布/plan话题
    void GoalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr goal) {
        RCLCPP_INFO(this->get_logger(), "Received goal pose: x = %f, y = %f", goal->pose.position.x,
                    goal->pose.position.y);

        // 等待动作服务器启动
        if (!path_action_client_->wait_for_action_server(std::chrono::seconds(10))) {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            return;
        }

        // 创建动作目标
        auto goal_msg       = nav2_msgs::action::ComputePathToPose::Goal();
        goal_msg.pose       = *goal;
        goal_msg.planner_id = "GridBased";

        // 发送动作目标
        auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::ComputePathToPose>::SendGoalOptions();
        send_goal_options.result_callback = std::bind(&GlobalPlanner::ResultCallback, this, std::placeholders::_1);
        auto future_goal_handle           = path_action_client_->async_send_goal(goal_msg, send_goal_options);
    }

    void ResultCallback(
        const rclcpp_action::ClientGoalHandle<nav2_msgs::action::ComputePathToPose>::WrappedResult &result) {
        switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED: {
                RCLCPP_INFO(this->get_logger(), "Path planning succeeded!");
                // 处理路径
                break;
            }
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "Path planning aborted");
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_ERROR(this->get_logger(), "Path planning canceled");
                break;
            default:
                RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                break;
        }
    }

    // 指向ComputePathToPose动作客户端
    rclcpp_action::Client<nav2_msgs::action::ComputePathToPose>::SharedPtr path_action_client_;
    // 用于接收目标姿态信息
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_subscription_;
    // 用于发布规划出的原始路径
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
};